#!python3

# This is a sample Python script.

# Press Shift+F10 to execute it or replace it with your code.
# Press Double Shift to search everywhere for classes, files, tool windows, actions, and settings.

from __future__ import print_function
import struct
import matplotlib.pyplot as plt
from plotly.subplots import make_subplots
import plotly.graph_objects as go
import sys
import numpy as np

a_X = []
a_Y = []
a_Z = []
g_X = []
g_Y = []
g_Z = []
temperature = []

if __name__ == '__main__':
    print("Input file is", sys.argv[1])   # "./data/capture.txt"
    FILE = open(sys.argv[1], "rb+")       # sys.argv[1]
    for i in range(124):
        a = FILE.read(1)
        if len(a) == 0:
            break
        temp = 0xbd
        packet_head_0 = temp.to_bytes(1, byteorder="big")
        if a == packet_head_0:
            b = FILE.read(1)
            if len(b) == 0:
                break
            temp = 0xdb
            packet_head_1 = temp.to_bytes(1, byteorder="big")
            if b == packet_head_1:
                c = FILE.read(1)
                if len(c) == 0:
                    break
                temp = 0x0a
                packet_head_2 = temp.to_bytes(1, byteorder="big")
                if c == packet_head_2:
                    rest = FILE.read(30)
                    if len(rest) < 30:
                        break
                    last = FILE.read(1)
                    if len(last) == 0:
                        break
                    # print(" ".join(hex(n) for n in a))
                    # print(" ".join(hex(n) for n in rest))
                    checksum = int(0)            # workaround
                    checksum = checksum ^ (a[0])
                    checksum = checksum ^ (b[0])
                    checksum = checksum ^ (c[0])
                    for j in rest:
                        checksum = checksum ^ j
                    # print(hex(checksum))
                    # print(hex(last[0]))
                    if checksum == last[0]:
                        # print("OK")
                        pos_g_x = 0
                        pos_g_y = pos_g_x + 4
                        pos_g_z = pos_g_y + 4
                        pos_a_x = pos_g_z + 4
                        pos_a_y = pos_a_x + 4
                        pos_a_z = pos_a_y + 4
                        pos_temp = pos_a_z + 4

                        g_x_array = rest[pos_g_x:pos_g_y]
                        temp = bytearray()   #
                        [temp.append(i) for i in reversed(g_x_array)]
                        g_X__ = struct.unpack("!f", temp)[0]   # ! means network byte order

                        g_y_array = rest[pos_g_y:pos_g_z]
                        temp = bytearray()   #
                        [temp.append(i) for i in reversed(g_y_array)]
                        g_Y__ = struct.unpack("!f", temp)[0]

                        g_z_array = rest[pos_g_z:pos_a_x]
                        temp = bytearray()   #
                        [temp.append(i) for i in reversed(g_z_array)]
                        g_Z__ = struct.unpack("!f", temp)[0]

                        a_x_array = rest[pos_a_x:pos_a_y]
                        temp = bytearray()   #
                        [temp.append(i) for i in reversed(a_x_array)]
                        a_X__ = struct.unpack("!f", temp)[0]

                        a_y_array = rest[pos_a_y:pos_a_z]
                        temp = bytearray()   #
                        [temp.append(i) for i in reversed(a_y_array)]
                        a_Y__ = struct.unpack("!f", temp)[0]

                        a_z_array = rest[pos_a_z:pos_temp]
                        temp = bytearray()   #
                        [temp.append(i) for i in reversed(a_z_array)]
                        a_Z__ = struct.unpack("!f", temp)[0]

                        temperature_array = rest[pos_temp:pos_temp+2]
                        temp = bytearray()   #
                        [temp.append(i) for i in temperature_array]
                        temp__ = struct.unpack("<H", temp)[0]        # < means little-endian byte order
                        temp__ = temp__*200.0/32768.0
                        # print("%f\n%f\n%f\n%f\n%f\n%f" % (g_X__, g_Y__, g_Z__, a_X__, a_Y__, a_Z__))

                        a_X.append(a_X__)
                        a_Y.append(a_Y__)
                        a_Z.append(a_Z__)
                        g_X.append(g_X__)
                        g_Y.append(g_Y__)
                        g_Z.append(g_Z__)
                        temperature.append(temp__)

    # plt.plot(a_X)
    # plt.show()
    # fig = go.Figure(data=go.Scatter(x=x, y=x ** 2))
    fig = make_subplots(rows=7, cols=1)
    fig.add_trace(go.Scatter(x=np.array(range(len(a_X))), y=np.array(a_X),
                             mode='lines',
                             name='acc_X'),
                  row=1, col=1)

    fig.add_trace(go.Scatter(x=np.array(range(len(a_Y))), y=np.array(a_Y),
                             mode='lines',
                             name='acc_Y'),
                  row=2, col=1)

    fig.add_trace(go.Scatter(x=np.array(range(len(a_Z))), y=np.array(a_Z),
                             mode='lines',
                             name='acc_Z'),
                  row=3, col=1)

    fig.add_trace(go.Scatter(x=np.array(range(len(g_X))), y=np.array(g_X),
                             mode='lines',
                             name='gyro_X'),
                  row=4, col=1)

    fig.add_trace(go.Scatter(x=np.array(range(len(g_Y))), y=np.array(g_Y),
                             mode='lines',
                             name='gyro_Y'),
                  row=5, col=1)

    fig.add_trace(go.Scatter(x=np.array(range(len(g_Z))), y=np.array(g_Z),
                             mode='lines',
                             name='gyro_Z'),
                  row=6, col=1)

    fig.add_trace(go.Scatter(x=np.array(range(len(temperature))), y=np.array(temperature),
                             mode='lines',
                             name='temperature'),
                  row=7, col=1)

    fig.update_layout(height=1600, width=1600, title_text="Subplots")
    fig.show()
